module mod_interp2
  ! translate from matlab interp2 function

  public init_interp2,interp2
  private
  real(8),allocatable,dimension(:)::s,t,t0,t1,t2,s0,s1,s2,zz
  integer,allocatable,dimension(:)::out,ndx,ndx2
  integer::nxd,nyd,ni,nout
  

contains

subroutine init_interp2(inyd,inxd,yd,xd,ini,yi,xi)
  implicit none
  integer::inxd,inyd,ini
  real(8)::xd(inxd)
  real(8)::yd(inyd)
  real(8)::xi(ini)
  real(8)::yi(ini)
  integer::i

  nxd=inxd
  nyd=inyd
  ni=ini

  allocate(s(ni));s=0D0
  allocate(t(ni));t=0D0
  allocate(t0(ni));t0=0D0
  allocate(t1(ni));t1=0D0
  allocate(t2(ni));t2=0D0
  allocate(s0(ni));s0=0D0
  allocate(s1(ni));s1=0D0
  allocate(s2(ni));s2=0D0
  allocate(zz((nxd+2)*(nyd+2)));zz=0D0
  allocate(out(ni));out=0
  allocate(ndx(ni));ndx=0
  allocate(ndx2(ni));ndx2=0

  s = 1D0 + (xi-xd(1))/(xd(nxd)-xd(1))*dble(nxd-1)
  t = 1D0 + (yi-yd(1))/(yd(nyd)-yd(1))*dble(nyd-1)

  nout=0
  do i=1,ni
     if( (s(i) .lt. 1D0) .or. (s(i) .gt. dble(nxd)) .or. (t(i) .lt. 1D0) .or. (t(i) .gt. dble(nyd))) then
        nout=nout+1
        out(nout)=i   
        if( (s(i) .lt. 1D0) .or. (s(i) .gt. dble(nxd))) then
           s(i)=1D0
        endif
        if( (t(i) .lt. 1D0) .or. (t(i) .gt. dble(nyd))) then
           t(i)=1D0
        endif
     endif
  enddo

  ndx = floor(t)+floor(s-1D0)*(nyd+2)

  do i=1,ni
     if(s(i) .eq. nxd) then 
        s(i) = (s(i) - floor(s(i)))
        s(i) = s(i)+1
        ndx(i) = ndx(i)-nyd-2
     else
        s(i) = (s(i) - floor(s(i)))
     endif
     if(t(i) .eq. nyd) then 
        t(i) = (t(i) - floor(t(i)))
        t(i) = t(i)+1D0
        ndx(i) = ndx(i)-1
     else
        t(i) = (t(i) - floor(t(i)))
     endif
  enddo

  t0 = ((2D0-t)*t-1D0)*t
  t1 = (3D0*t-5D0)*t*t+2D0
  t2 = ((4-3*t)*t+1)*t
  t = (t-1)*t*t
  s0=(((2D0-s)*s-1D0)*s)
  s1= ((3D0*s-5D0)*s*s+2D0)
  s2=((4D0-3D0*s)*s+1D0)*s
  s=((s-1D0)*s*s)

end subroutine init_interp2

subroutine interp2(zd,zi)
  ! zd is a grid data, zi is scatter data
  implicit none
  real(8)::zi(ni)
  real(8)::zd(nyd,nxd)
  ndx2=ndx
  call interp2_helper(nxd,nyd,zz,zd)
  zi  = ( zz(ndx)*t0 + zz(ndx+1)*t1 + zz(ndx+2)*t2 + zz(ndx+3)*t )* s0
  ndx = ndx + nyd +2
  zi  = zi + ( zz(ndx)*t0 + zz(ndx+1)*t1 + zz(ndx+2)*t2 + zz(ndx+3)*t ) * s1
  ndx = ndx + nyd +2
  zi  = zi + ( zz(ndx)*t0 + zz(ndx+1)*t1 + zz(ndx+2)*t2 + zz(ndx+3)*t )* s2
  ndx = ndx + nyd +2
  zi  = zi + ( zz(ndx)*t0 + zz(ndx+1)*t1 + zz(ndx+2)*t2 + zz(ndx+3)*t ) * s
  zi  = zi/4D0
  zi(out(1:nout)) = 0D0
  ndx=ndx2
end subroutine interp2

subroutine interp2_helper(nxd,nyd,zz,zd)
  implicit none
  integer::nxd,nyd
  real(8)::zz(nyd+2,nxd+2)
  real(8)::zd(nyd,nxd)
  zz(1,2:nxd+1) = 3*zd(1,:)-3*zd(2,:)+zd(3,:);
  zz(2:nyd+1,2:nxd+1) = zd
  zz(nyd+2,2:nxd+1) = 3*zd(nyd,:)-3*zd(nyd-1,:)+zd(nyd-2,:)
  zz(:,1) = 3*zz(:,2)-3*zz(:,3)+zz(:,4)
  zz(:,nxd+2) = 3*zz(:,nxd+1)-3*zz(:,nxd)+zz(:,nxd-1)
end subroutine interp2_helper

end module mod_interp2

